#ifndef _SERVO_ZAPI_H__
#define _SERVO_ZAPI_H__

#include <motor/motor.h>
#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/int16.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <std_msgs/msg/u_int32.hpp>
#include <agv_msgs/msg/periph_custom_operation.hpp>
#include <encoder/encoder_factory.h>


namespace ros_canopen_driver {

enum PerphOperations
{
    STOP = 0,
    PICKUP = 1,
    PUTDOWN = 2
};

struct PDO_CACHE_ZAPI
{
    std::atomic<uint16_t>   error_code{0};
    std::atomic<int16_t>    current_vel{0};
    std::atomic<uint16_t>   digital_input{0};
    std::atomic<double>     timestamp{0.0};
};

class ServoZapi : public Motor
{
public:
    ServoZapi(std::shared_ptr<rclcpp::Node> node, const std::string name);
    ~ServoZapi();

    int Motor_Init();

    bool Motor_Enable(bool onFlag);

    bool Motor_InitParam();

    bool Motor_InitVmode();

    bool Motor_InitPmode();

    bool Motor_HomeMachine();

    bool Motor_SwitchMode(int8_t mode);


    void Motor_SetAcc(uint32_t acc);

    void Motor_SetDec(uint32_t dec);

    void Motor_SetPmodeVel(uint32_t vel);

    void Motor_SetTargetVelRad(double vel);

    void Motor_SetTargetVelEnc(int vel);

    void Motor_SetTargetPosRad(double pos);


    MotorState Motor_GetState();

    double Motor_GetCurrentVel();

    double Motor_GetCurrentPos();

    double Motor_GetCurrent();

    uint16_t Motor_GetStatusWord();

    uint16_t Motor_GetErrCode();

    int8_t Motor_GetMode();


    void Motor_Brake(bool on);

    void Motor_Shutdown();

    bool Motor_Halt();

    bool Motor_Recover();

    double Motor_GetTimestamp() { return pc_.timestamp;}

    std::string Motor_Err2userErr(const uint16_t& motorErr);


private:
    SERVOMOTOR_PORT m_servo_motor;
    std::atomic<MotorState> motorState;

    struct PDO_CACHE_ZAPI pc_;
    struct ForkLiftParam fork_param;

    // rpdo pub
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt16>> set_vel_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_control_word_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_io_output_pub;
    
    // tpdo sub
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr current_vel_sub;
    rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr errcode_sub;
    rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr di_sub;

    // interfaces
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt16>> di_interface_pub;
    rclcpp::Subscription<agv_msgs::msg::PeriphCustomOperation>::SharedPtr custom_operation_sub;

    std::shared_ptr<encoder::Encoder> encoder;

    void servo_VvelSet(int vel);

    int servo_velGet();

    uint32_t servo_PvelGet();

    int servo_VvelGet();

    void servo_doForkLift(int opt);


    void current_vel_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);
    void errcode_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg);
    void di_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg);
    void custom_operation_sub_callback(const agv_msgs::msg::PeriphCustomOperation::ConstSharedPtr& msg);
};

}
#endif
